#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     HTPB,                sensorI2CCustom9V)
#pragma config(Sensor, S3,     HTAC,               sensorI2CCustom)
#pragma config(Sensor, S4,     SMUX_2,              sensorI2CCustom)
#pragma config(Motor,  motorA,          ,              tmotorNormal, openLoop, encoder)
#pragma config(Motor,  motorB,          ,              tmotorNormal, openLoop, encoder)
#pragma config(Motor,  motorC,          legomc,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     a,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     b,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     c,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     d,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     e,             tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     f,             tmotorNormal, openLoop, encoder)
#pragma config(Servo,  srvo_S1_C4_1,    serva,                tServoStandard)

task main {
  nMotorEncoder[a] = 0;
  nMotorEncoder[b] = 0;
  nMotorEncoder[c] = 0;
  nMotorEncoder[d] = 0;
  while (true){
    nxtDisplayTextLine(1, "Encoder a: %d", nMotorEncoder[a]);
    nxtDisplayTextLine(2, "Encoder b: %d", nMotorEncoder[b]);
    nxtDisplayTextLine(3, "Encoder c: %d", nMotorEncoder[c]);
    nxtDisplayTextLine(4, "Encoder d: %d", nMotorEncoder[d]);
  }
}
